#include "pid.h"

#define MaxVal(a, b)         (((a) >= (b)) ? (a) : (b))
#define MinVal(a, b)         (((a) >= (b)) ? (b) : (a))
#define SubVal(a, sa, b, sb) (((sa) != (sb)) ? ((a) + (b)) : (((a) >= (b)) ? ((a) - (b)) : ((b) -  (a))))
#define SubSig(a, sa, b, sb) (((sa) != (sb)) ? (sa)        : (((a) >= (b)) ? (sa)        : ( 1  - (sa))))
#define AddVal(a, sa, b, sb) (((sa) == (sb)) ? ((a) + (b)) : (((a) >= (b)) ? ((a) - (b)) : ((b) -  (a))))
#define AddSig(a, sa, b, sb) (((sa) == (sb)) ? (sa)        : (((a) >= (b)) ? (sa)        : ( 1  - (sa))))

#define MOTOR_NUM   2

uint32_t         pid_kp[MOTOR_NUM];                 // KP     << 10   : 1 : 0.001 : [0 - 1024]
uint32_t         pid_ki[MOTOR_NUM];                 // KI     << 10   : 1 : 0.001 : [0 - 1024]
uint32_t         pid_kd[MOTOR_NUM];                 // KD     << 10   : 1 : 0.001 : [0 - 1024]
uint32_t         pid_ek[MOTOR_NUM];                 // e(k)           :
uint8_t          pid_sek[MOTOR_NUM];                // sign of e(k)   :
uint32_t         pid_ekl[MOTOR_NUM];                // e(k-1)         :
uint8_t          pid_sekl[MOTOR_NUM];               // sign of e(k-1) :
uint32_t         pid_ekp[MOTOR_NUM];                // e(k-2)         :
uint8_t          pid_sekp[MOTOR_NUM];               // sign of e(k-2) :
uint32_t         pid_ei[MOTOR_NUM];                 // ?e(k)          :
uint8_t          pid_sei[MOTOR_NUM];                // sign of ?e(k)  :
uint32_t         pid_u[MOTOR_NUM];                  // u(k)   << 10   : 1 : 0.001
uint8_t          pid_su[MOTOR_NUM];                 // sign of u(k)   :
uint32_t         pid_umax[MOTOR_NUM];               // max of u(k)    : 1 : 0.001
uint32_t         pid_du[MOTOR_NUM];                 // du     << 10   : 1 : 0.001
uint8_t          pid_sdu[MOTOR_NUM];                // sign of du     :
PID_METHOD       pid_method[MOTOR_NUM];             // Position Method or Increment Method

static void PidPosition(uint8_t motor, int32_t in_cmd, int32_t in_fbk);

void PidPosition(uint8_t motor, int32_t in_cmd, int32_t in_fbk) {
  uint32_t cmd    = (in_cmd >= 0) ? (in_cmd) : (-in_cmd);
  uint8_t scmd    = (in_cmd >= 0) ? (0)      : (1)      ;
  uint32_t fbk    = (in_fbk >= 0) ? (in_fbk) : (-in_fbk);
  uint8_t sfbk    = (in_fbk >= 0) ? (0)      : (1)      ;
  pid_ek[motor]   =        SubVal(cmd, scmd, fbk, sfbk);
  pid_sek[motor]  =        SubSig(cmd, scmd, fbk, sfbk);
  pid_ei[motor]   = MinVal(AddVal(pid_ei[motor], pid_sei[motor], pid_ek[motor], pid_sek[motor]), pid_umax[motor] >> 10);
  pid_sei[motor]  =        AddSig(pid_ei[motor], pid_sei[motor], pid_ek[motor], pid_sek[motor]);
  uint32_t p      = pid_kp[motor] * pid_ek[motor];
  uint32_t i      = pid_ki[motor] * pid_ei[motor];
  uint32_t d      = pid_kd[motor] * SubVal(pid_ek[motor], pid_sek[motor], pid_ekl[motor], pid_sekl[motor]);
  uint32_t sd     =        SubSig(pid_ek[motor], pid_sek[motor], pid_ekl[motor], pid_sekl[motor]);
  pid_u[motor]    =        AddVal(p, pid_sek[motor], i, pid_sei[motor]);
  pid_su[motor]   =        AddSig(p, pid_sek[motor], i, pid_sei[motor]);
  pid_u[motor]    = MinVal(AddVal(pid_u[motor], pid_su[motor], d, sd), pid_umax[motor]);
  pid_su[motor]   =        AddSig(pid_u[motor], pid_su[motor], d, sd);
  pid_ekl[motor]  = pid_ek[motor];
  pid_sekl[motor] = pid_sek[motor];
}

/*
void PidIncrement(int32_t in_cmd, int32_t in_fbk) {
  uint32_t cmd   = (in_cmd >= 0) ? (in_cmd) : (-in_cmd);
  uint8_t scmd   = (in_cmd >= 0) ? (0)      : (1)      ;
  uint32_t fbk   = (in_fbk >= 0) ? (in_fbk) : (-in_fbk);
  uint8_t sfbk   = (in_fbk >= 0) ? (0)      : (1)      ;
  pid_ek         = SubVal(cmd, scmd, fbk, sfbk);
  pid_sek        = SubSig(cmd, scmd, fbk, sfbk);
  uint32_t p     = pid_kp * SubVal(pid_ek, pid_sek, pid_ekl, pid_sekl);
  uint32_t sp    =          SubSig(pid_ek, pid_sek, pid_ekl, pid_sekl);
  uint32_t i     = pid_ki * pid_ek;
  uint32_t d     =          SubVal(pid_ek, pid_sek, (pid_ekl << 1), pid_sekl);
  uint32_t sd    =          SubSig(pid_ek, pid_sek, (pid_ekl << 1), pid_sekl);
  d              = pid_kd * AddVal(d, sd, pid_ekp, pid_sekp);
  sd             =          AddSig(d, sd, pid_ekp, pid_sekp);
  pid_du         = AddVal(p, sp, i, pid_sek);
  pid_sdu        = AddSig(p, sp, i, pid_sek);
  pid_du         = AddVal(pid_du, pid_sdu, d, sd);
  pid_sdu        = AddSig(pid_du, pid_sdu, d, sd);
  pid_ekp        = pid_ekl;
  pid_sekp       = pid_sekl;
  pid_ekl        = pid_ek;
  pid_sekl       = pid_sek;
}
*/

void PID_Init() {
  pid_kp[0]       = 1000;                 // KP     << 10   : 1 : 0.001 : [0 - 1024]
  pid_ki[0]       = 10;                   // KI     << 10   : 1 : 0.001 : [0 - 1024]
  pid_kd[0]       = 20;                   // KD     << 10   : 1 : 0.001 : [0 - 1024]
  pid_ek[0]       = 0;                    // e(k)           :
  pid_sek[0]      = 0;                    // sign of e(k)   :
  pid_ekl[0]      = 0;                    // e(k-1)         :
  pid_sekl[0]     = 0;                    // sign of e(k-1) :
  pid_ekp[0]      = 0;                    // e(k-2)         :
  pid_sekp[0]     = 0;                    // sign of e(k-2) :
  pid_ei[0]       = 0;                    // ∑e(k)          :
  pid_sei[0]      = 0;                    // sign of ∑e(k)  :
  pid_u[0]        = 0;                    // u(k)   << 10   : 1 : 0.001
  pid_su[0]       = 0;                    // sign of u(k)   :
  pid_umax[0]     = 1999 << 10;           // max of u(k)    : 1 : 0.001
  pid_du[0]       = 0;                    // du     << 10   : 1 : 0.001
  pid_sdu[0]      = 0;                    // sign of du     :
	pid_method[0]   = PID_POSITION_METHOD;  // Position Method or Increment Method

	pid_kp[1]       = 1000;                 // KP     << 10   : 1 : 0.001 : [0 - 1024]
  pid_ki[1]       = 10;                   // KI     << 10   : 1 : 0.001 : [0 - 1024]
  pid_kd[1]       = 20;                   // KD     << 10   : 1 : 0.001 : [0 - 1024]
  pid_ek[1]       = 0;                    // e(k)           :
  pid_sek[1]      = 0;                    // sign of e(k)   :
  pid_ekl[1]      = 0;                    // e(k-1)         :
  pid_sekl[1]     = 0;                    // sign of e(k-1) :
  pid_ekp[1]      = 0;                    // e(k-2)         :
  pid_sekp[1]     = 0;                    // sign of e(k-2) :
  pid_ei[1]       = 0;                    // ∑e(k)          :
  pid_sei[1]      = 0;                    // sign of ∑e(k)  :
  pid_u[1]        = 0;                    // u(k)   << 10   : 1 : 0.001
  pid_su[1]       = 0;                    // sign of u(k)   :
  pid_umax[1]     = 1999 << 10;           // max of u(k)    : 1 : 0.001
  pid_du[1]       = 0;                    // du     << 10   : 1 : 0.001
  pid_sdu[1]      = 0;                    // sign of du     :
	pid_method[1]   = PID_POSITION_METHOD;  // Position Method or Increment Method
}

void PID_Calculate(uint8_t motor, int32_t in_cmd, int32_t in_fbk, uint16_t* out_u, uint8_t* out_su) {
	PidPosition(motor, in_cmd, in_fbk);
	*out_u          = pid_u[motor] >> 10;
	*out_su         = pid_su[motor];
}
